#ifndef __CONTROL_H
#define __CONTROL_H

#include "main.h"
#include <stdint.h>
#include "motor.h"
#include "encoder_module.h"

/* PID参数结构体 */
typedef struct {
    float Kp;           // 比例系数
    float Ki;           // 积分系数
    float Kd;           // 微分系数
    float target;       // 目标值
    float error;        // 当前误差
    float last_error;   // 上一次误差
    float integral;     // 积分累积
    float integral_max; // 积分限幅
    float output_max;   // 输出限幅
    float output_min;   // 输出下限
    float output;       // PID输出
} pid_controller_t;

/* 电机控制模式 */
typedef enum {
    MOTOR_MODE_SPEED,     // 速度控制模式
    MOTOR_MODE_POSITION   // 位置控制模式
} motor_mode_t;

/* 电机控制结构体 */
typedef struct {
    pid_controller_t speed_pid;     // 速度PID控制器
    pid_controller_t position_pid;  // 位置PID控制器
    motor_mode_t mode;              // 控制模式
    encoder_handle_t encoder;       // 编码器句柄
    int16_t current_speed;          // 当前速度
    float current_position;         // 当前位置
    int16_t output_speed;           // 输出速度
} motor_controller_t;

/* 双电机控制器 */
typedef struct {
    motor_controller_t motor1;      // 电机1控制器
    motor_controller_t motor2;      // 电机2控制器
    uint8_t enable;                 // 使能标志
} dual_motor_controller_t;


/* 函数声明 */

/* PID初始化函数 */
void PID_Init(pid_controller_t *pid, float kp, float ki, float kd, 
              float integral_max, float output_max, float output_min);

/* PID参数设置函数 */
void PID_SetParams(pid_controller_t *pid, float kp, float ki, float kd);

/* PID目标值设置函数 */
void PID_SetTarget(pid_controller_t *pid, float target);

/* PID计算函数 */
float PID_Calculate(pid_controller_t *pid, float current_value);

/* PID重置函数 */
void PID_Reset(pid_controller_t *pid);

/* 双电机控制器初始化 */
void DualMotor_Init(dual_motor_controller_t *controller, 
                    encoder_handle_t encoder1, encoder_handle_t encoder2);

/* 设置电机控制模式 */
void DualMotor_SetMode(dual_motor_controller_t *controller, motor_mode_t mode);

/* 设置速度目标值 */
void DualMotor_SetSpeedTarget(dual_motor_controller_t *controller, 
                              int16_t speed1, int16_t speed2);

/* 设置位置目标值 */
void DualMotor_SetPositionTarget(dual_motor_controller_t *controller, 
                                 float position1, float position2);

/* 双电机控制主函数（需要在定时器中断或主循环中定期调用） */
void DualMotor_Control(dual_motor_controller_t *controller);

/* 启动/停止电机控制 */
void DualMotor_Enable(dual_motor_controller_t *controller, uint8_t enable);

/* 设置速度PID参数 */
void DualMotor_SetSpeedPID(dual_motor_controller_t *controller, uint8_t motor_id,
                          float kp, float ki, float kd);

/* 设置位置PID参数 */
void DualMotor_SetPositionPID(dual_motor_controller_t *controller, uint8_t motor_id,
                             float kp, float ki, float kd);

#endif /* __CONTROL_H */
